Hovercraft Path Planning

In this case study, we seek to determine an optimal control policy for the trajectory of a hovercraft that travels to a set of dynamic waypoints while trying to minimize the thrust input.

Formulation

The corresponding dynamic optimization problem is expressed:

\[\begin{aligned} &&\underset{x(t), v(t), u(t)}{\text{min}} &&& \int_{t \in T} |u(t)|_2^2 dt \\ &&\text{s.t.} &&& v(0) = v0\\ &&&&& \frac{dx}{dt} = v(t), && t \in T\\ &&&&& \frac{dv}{dt} = u(t), && t \in T\\ &&&&& x(t_i) = xw_i, && i \in I \end{aligned}\]

where $x(t)$ is the Cartesian position, $v(t)$ is the velocity, $u(t)$ is the thrust input, $xw_i, \ i \in I,$ are the waypoints, and $T$ is the time horizon.

Model Definition

Let's implement this in InfiniteOpt and first import the packages we need:

using InfiniteOpt, Ipopt

Next we'll specify our waypoint data:

xw = [1 4 6 1; 1 3 0 1] # positions
tw = [0, 25, 50, 60];    # times

We initialize the infinite model and opt to use the Ipopt solver:

m = InfiniteModel(optimizer_with_attributes(Ipopt.Optimizer, "print_level" => 0));

Let's specify our infinite parameter which is time $t \in [0, 60]$:

@infinite_parameter(m, t in [0, 60], num_supports = 61)

Now let's specify the decision variables:

@variables(m, begin
    # state variables
    x[1:2], Infinite(t)
    v[1:2], Infinite(t)
    # control variables
    u[1:2], Infinite(t), (start = 0)
end)

(GeneralVariableRef[x[1](t), x[2](t)], GeneralVariableRef[v[1](t), v[2](t)], GeneralVariableRef[u[1](t), u[2](t)])

Specify the objective:

@objective(m, Min, ∫(u[1]^2 + u[2]^2, t))

∫{t ∈ [0, 60]}[u[1](t)² + u[2](t)²]

Set the initial conditions with respect to the velocity:

@constraint(m, [i = 1:2], v[i](0) == 0)

2-element Vector{InfOptConstraintRef}:
 v[1](0) = 0
 v[2](0) = 0

Define the point physics ODEs which serve as our system model:

@constraint(m, [i = 1:2], ∂(x[i], t) == v[i])
@constraint(m, [i = 1:2], ∂(v[i], t) == u[i])

2-element Vector{InfOptConstraintRef}:
 d/dt[v[1](t)] - u[1](t) = 0, ∀ t ∈ [0, 60]
 d/dt[v[2](t)] - u[2](t) = 0, ∀ t ∈ [0, 60]

Ensure we hit all the waypoints:

@constraint(m, [i = 1:2, j = eachindex(tw)], x[i](tw[j]) == xw[i, j])

2×4 Matrix{InfOptConstraintRef}:
 x[1](0) = 1  x[1](25) = 4  x[1](50) = 6  x[1](60) = 1
 x[2](0) = 1  x[2](25) = 3  x[2](50) = 0  x[2](60) = 1

Problem Solution

Optimize the model:

optimize!(m)

Extract the results. The InfiniteInterpolations extension can be used to get a smooth interpolated function for x, which is invoked when both the Interpolations and InfiniteOpt packages are imported. Here, cubic splines are used as the interpolation method for both x1 and x2:

using Interpolations
xFunc = value.(x, cubic_spline_interpolation);

Query our interpolated function for the values of x1 and x2:

tvals = LinRange(0, 60, 100)
x1Vals = xFunc[1](tvals)
x2Vals = xFunc[2](tvals);

Plot the results:

using Plots
scatter(xw[1,:], xw[2,:], label = "Waypoints")
xlabel!("x_1")
ylabel!("x_2")
plot!(x1Vals, x2Vals, label = "Trajectory")

That's it, now we have our optimal trajectory!

Maintenance Tests

These are here to ensure this example stays up to date.

using Test
tol = 1E-6
@test termination_status(m) == MOI.LOCALLY_SOLVED
@test has_values(m)
@test x1Vals isa Vector{<:Real}
@test x2Vals isa Vector{<:Real}
@test isapprox(objective_value(m), 0.043685293177035435, atol=tol)
@test isapprox(value(u[1])[end], -0.010503853944039986, atol=tol)
@test isapprox(value(u[2])[end], 0.005456780217220367, atol=tol)
@test isapprox(x1Vals[15], 1.3837274935883543, atol=tol)
@test isapprox(x2Vals[15], 1.6386021193421085, atol=tol)

Test Passed

This page was generated using Literate.jl.